#include <functional>
#include <memory>
#include <thread>
#include <queue>
#include <sstream>

#include "rclcpp/rclcpp.hpp"
#include "test_interfaces/srv/test.hpp"

using std::placeholders::_1;
using std::placeholders::_2;
using namespace std::chrono_literals;

class TestClient : public rclcpp::Node
{
private:
    int sum = 0;
    bool next_goal = true;
    std::queue<int> request_list;

    rclcpp::CallbackGroup::SharedPtr callback_group_service;
    rclcpp::Client<test_interfaces::srv::Test>::SharedPtr client_;
    rclcpp::TimerBase::SharedPtr timer_;

public:
    TestClient();
    ~TestClient();
    void start();
    void my_loop();
    void send_request();
    void callback(rclcpp::Client<test_interfaces::srv::Test>::SharedFuture future);
};

TestClient::TestClient() : Node("test_client")
{
    client_ = this->create_client<test_interfaces::srv::Test>("add_three_ints");
    this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&TestClient::my_loop, this));
    
    // 构造请求队列
    this->request_list.push(2);
    this->request_list.push(2);
}

TestClient::~TestClient()
{
}

void TestClient::start()
{
    RCLCPP_INFO(this->get_logger(), "start");
}

// 定时器回调
void TestClient::my_loop()
{
    if (this->next_goal && !this->request_list.empty())
    {
        this->send_request();
    }
    else if (this->request_list.empty())
    {
        rclcpp::shutdown();
    }
    else
    {
        return;
    }
}

void TestClient::send_request()
{
    // 暂停后续请求
    this->next_goal = false;

    // 发送此次请求
    auto request = std::make_shared<test_interfaces::srv::Test::Request>();
    request->a = 1;
    request->b = 2;
    request->c = 3;

    client_->async_send_request(request, std::bind(&TestClient::callback, this, _1));
    RCLCPP_INFO(this->get_logger(), "sended request");
}

void TestClient::callback(rclcpp::Client<test_interfaces::srv::Test>::SharedFuture future)
{
    // 处理请求结果
    auto result = future.get();
    this->sum = result->sum;
    RCLCPP_INFO(this->get_logger(), "got response");
    RCLCPP_INFO(this->get_logger(), "result is %d", this->sum);

    // 放开后续请求
    this->request_list.pop();
    this->next_goal = true;
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);

    auto client = std::make_shared<TestClient>();
    client->start();

    rclcpp::executors::MultiThreadedExecutor exe;
    exe.add_node(client);
    exe.spin();

    rclcpp::shutdown();
    return 0;
}